#include "postprocess/post_processor.h"

#include <filesystem>

#include "common/basic_settings.h"
#include "common/file_helper.h"
#include "postprocess/common/postprocess_id_generator.h"

PostProcessor::PostProcessor() noexcept : is_init_(false) {}

PostProcessor::~PostProcessor() {}

PostProcessor &PostProcessor::GetInstance() noexcept {
  static PostProcessor instance;
  return instance;
}

void PostProcessor::Init() noexcept(false) {
  std::lock_guard<std::mutex> lock(mutex_);
  if (!is_init_) {
    auto &id_generator =
        PostProcessIdGenerator::GetInstance();  // init singleton of IdGenerator
    id_generator.Init();
    // create path
    auto const &configRootPath{autodrive::perception::sConfigRootPath};
    if (!std::filesystem::exists(configRootPath)) {
      if (!std::filesystem::create_directories(configRootPath)) {
        throw std::runtime_error("create directory " + configRootPath +
                                 " fail");
      }
    }

    std::string configFileName{"PostProcessSettings.json"};
    auto const &configFile{std::filesystem::path(configRootPath) /
                           std::filesystem::path(configFileName)};
    if (!std::filesystem::exists(configFile)) {
      // use default settings
      auto const &defaultSettings{gen_default()};
      auto const &sjSettings{
          autodrive::perception::serialize_json(defaultSettings)};
      autodrive::perception::write_json(configFile.string(), sjSettings);
    }

    auto const &sSettings{autodrive::perception::read_text(configFile)};
    settings_ = autodrive::perception::json_deserialize<PostProcessorSettings>(
        sSettings);

    vehicle_processor_.Init(settings_);     // init vehicle processor
    pedestrian_processor_.Init(settings_);  // init pedestrian processor
    pre_pose_.matrix() = Eigen::Matrix4d::Identity();
    cur_pose_.matrix() = Eigen::Matrix4d::Identity();
  }

  is_init_ = true;
}

bool PostProcessor::Process(
    autodrive::perception::PerceptionResult &perceptionResult,
    Eigen::Affine3d &pos, std::string &err) noexcept(false) {
  if (!is_init_) {
    err = "post processor module must do init  at first!";
    return false;
  }

  cur_pose_ = pos;

  if (vehicle_processor_.Run(perceptionResult, pos, err) &&
      pedestrian_processor_.Run(perceptionResult, pos, err)
      //&& static_processor_.Run(perceptionResult, err)
  ) {
    // collect  processor output
    // 1、 clear perception result
    perceptionResult.output_objects.clear();

    // 2、collect vehicle processor result
    ObjectTrackSet &vehicle_objecttrackset =
        vehicle_processor_.GetObjectTrackSet();
    collect_tracked_results(perceptionResult, vehicle_objecttrackset);

    // 3、collect pedestrian processor result
    ObjectTrackSet &pedestrian_objecttrackset =
        pedestrian_processor_.GetObjectTrackSet();
    collect_tracked_results(perceptionResult, pedestrian_objecttrackset);

    pre_pose_ = cur_pose_;

    return true;
  } else {
    err = "Error happens in post processor, please check!";
    pre_pose_ = cur_pose_;
    return false;
  }
}

PostProcessorSettings PostProcessor::gen_default() noexcept {
  // default settings
  PostProcessorSettings settings{
      .track_cached_history_size_maximum = 5,
      .track_acceleration_noise_maximum = 0.6f,
      .track_speed_noise_maximum = 5.0f,
      .collect_track_consecutive_invisible_maximum = 4,
      .collect_track_age_minimum = 5,
      .initial_position_noise = 0.5f,
      .initial_velocity_noise = 0.5f,
      .initial_acceleration_noise = 1.0f,
      .initial_env_noiose = 0.5,
      .vehicle_2diou_threshold = 0.1f,
      .vehicle_distance_threshold = 1.8f,
      .pedestrian_2diou_threshold = 0.1f,
      .pedestrian_distance_threshold = 1.8f,
      .use_kalman_breakdown = true,
      .vehicle_distance_breakdown_threshold = 1.3f,
      .vehicle_velocity_breakdown_threshold = 0.3f,
      .vehicle_acceleration_breakdown_threshold = 1.0f,
      .pedestrian_distance_breakdown_threshold = 1.0f,
      .pedestrian_velocity_breakdown_threshold = 0.2f,
      .pedestrian_acceleration_breakdown_threshold = 1.0f,
      .motion_vehicle_Q{0.046f, 0.0f, 0.0f, 0.0f, 0.0f,  0.0f,  0.0f,  0.040f,
                        0.0f,   0.0f, 0.0f, 0.0f, 0.0f,  0.0f,  0.08f, 0.0f,
                        0.0f,   0.0f, 0.0f, 0.0f, 0.0f,  0.30f, 0.0f,  0.0f,
                        0.0f,   0.0f, 0.0f, 0.0f, 0.02f, 0.0f,  0.0f,  0.0f,
                        0.0f,   0.0f, 0.0f, 0.05f},
      .motion_vehicle_R{0.08f, 0.0f, 0.0f, 0.0f, 0.0f, 0.024f, 0.0f, 0.0f, 0.0f,
                        0.0f, 0.55f, 0.0f, 0.0f, 0.0f, 0.0f, 0.18f},
      .motion_pedestrian_Q{
          0.046f * 5, 0.0f,  0.0f, 0.0f,  0.0f, 0.0f,  0.0f, 0.040f, 0.0f,
          0.0f,       0.0f,  0.0f, 0.0f,  0.0f, 0.08f, 0.0f, 0.0f,   0.0f,
          0.0f,       0.0f,  0.0f, 0.30f, 0.0f, 0.0f,  0.0f, 0.0f,   0.0f,
          0.0f,       0.02f, 0.0f, 0.0f,  0.0f, 0.0f,  0.0f, 0.0f,   0.05f},
      .motion_pedestrian_R{0.08f, 0.0f, 0.0f, 0.0f, 0.0f, 0.024f, 0.0f, 0.0f,
                           0.0f, 0.0f, 0.55f, 0.0f, 0.0f, 0.0f, 0.0f, 0.18f}};

  return settings;
}

void PostProcessor::collect_tracked_results(
    autodrive::perception::PerceptionResult &perceptionResult,
    ObjectTrackSet &object_tracks_set) noexcept {
  const std::vector<ObjectTrackPtr> &tracks = object_tracks_set.GetTracks();
  for (std::uint32_t i{0}; i < tracks.size(); ++i) {
    TrackedObjectPtr result_obj{tracks[i]->current_object_};
    if (tracks[i]->consecutive_invisible_count_ >
        settings_.collect_track_consecutive_invisible_maximum) {
      continue;
    }

    if (tracks[i]->age_ < settings_.collect_track_age_minimum) {
      continue;
    }
    // 将数值赋予输出结构体
    if (perceptionResult.output_objects.full()) {
      break;
    }
    auto obs{perceptionResult.output_objects.end()};
    obs->id = tracks[i]->idx_;
    obs->center = result_obj->center;
    obs->size = result_obj->size;
    // heading使用一阶滤波器
    //    obs.heading = result_obj->heading;
    Eigen::Vector3f heading_vec{0.0f, 0.0f, result_obj->heading};
    result_obj->heading_filter.AddMeasurement(heading_vec, 1.0f);
    obs->heading = result_obj->heading_filter.GetState().z();
    obs->velocity = result_obj->velocity;
    obs->acceleration = result_obj->acceleration;
    // obs.label = result_obj->label;
    obs->label = tracks[i]->fused_type_;

    perceptionResult.output_objects.resize(
        perceptionResult.output_objects.size() + 1);
  }
}
